// File:          my_controller.cpp
// Date:
// Description:
// Author:  baolan
// Modifications:

// You may need to add webots include files such as
// <webots/DistanceSensor.hpp>, <webots/Motor.hpp>, etc.
// and/or to add some other includes
#include <motionControl.hpp>
#include <robot_locomotion.h>
#include <ctime>
#include <fstream>
#define PI 3.1415926

// All the webots classes are defined in the "webots" namespace
using namespace webots;
MotionControl mc;
rl rl1;

int main(int argc, char **argv) {
    Robot *robot = new Robot();
    Keyboard kb;
    kb.enable(1000);
    float timePeriod = 0.01;
    mc.MotionContr(timePeriod, robot);

    int timeStep = (int)robot->getBasicTimeStep();

    Matrix<float, 4, 3> initPos;
    float body_x_del = -0.01;//0.038
    initPos<<  body_x_del, 0.0, -mc.bodyh, body_x_del, 0.0, -mc.bodyh, body_x_del, 0.0, -mc.bodyh, body_x_del, 0.0, -mc.bodyh;
    mc.setInitPos(initPos);
    mc.tCV<< 0.00, 0.0, 0.0;
    //w:87,s:83,a:65,d:68，q:81,e:69
    mc.setCoMVel(mc.tCV);
    char StateVal;
    StateVal = mc.stateval[0];

    ofstream oFile;
    oFile.open("distance.csv",ios::out | ios::trunc);
  while (robot->step(timeStep) != -1)
  {
    mc.key = kb.getKey();
    mc.Sensor_update();
    mc.forwardKinematics();
    mc.jacobians();
    mc.state_judgement();

    rl1.dsensor[0] =  mc.distance[0];
    rl1.dsensor[1] =  mc.distance[1];
    rl1.dsensor[2] =  mc.distance[2];
    rl1.dsensor[3] =  mc.distance[3];
    rl1.dsensor[4] =  mc.distance[4];
    rl1.dsensor[5] =  mc.distance[5];

    rl1.run();
    rl1.update();
    
    switch(StateVal)
    {
      case 'a'://
        mc.State_start();
        if (mc.Start_time >= 0.40)
        {
          StateVal = mc.stateval[1];
          mc.fly_time = 0.0;
          mc.stateFlag << 1, 0, 0, 1;
        }
        break;
      case 'b':// 2 leg stance
        // mc.swing();
        mc.nextStep();
        mc.del_imu();
        mc.statechange();
        mc.inverseKinematics();
        mc.Setjoint();
        static int temp_num = 0;
        if (mc.change_judge == 1 && temp_num <= 100)
        {
          mc.tCV<< 0.00, 0.0, 0.0;
          temp_num += 1;
        }
        else if (mc.change_judge == 1 && mc.fly_time >= mc.T && temp_num >= 100)
        {
          StateVal = mc.stateval[2];
          temp_num = 0;
          mc.fly_time = 0.0;
        }
        break;
      case 'c'://越障
        mc.forwardKinematics();
        mc.changestate();
        mc.inverseKinematics();
        mc.Setjoint();
        if(mc.surmountFlag == 3 && mc.fly_time > mc.change_T + 0.005)
        {
            StateVal = mc.stateval[3];
            if (mc.change_judge == 0)
            mc.change_judge = 1;
            else
            mc.change_judge = 0;
        }
        if(mc.fly_time > mc.change_T + 0.005)
        {
            StateVal = mc.stateval[3];
            mc.surmountFlag += 1;
            mc.fly_time = 0.0;
            if (mc.surmountFlag > 3)
            {
                mc.surmountFlag = 0;
                mc.change_stop = 1;
            }
        }
        break;
      case 'd'://
        mc.forwardKinematics();
        mc.stance_4leg();
        mc.inverseKinematics();
        static int temp_d = 0;
        if (temp_d == 40)
        {
            if (mc.change_stop == 1)
            {
                StateVal = mc.stateval[1];
                mc.change_stop = 0;
            }
            else
            {
                StateVal = mc.stateval[2];
            }
            temp_d = 0;
        }
        temp_d += 1;
        break;
      default:
      break;
    }
    cout << "v:   "<<mc.tCV.transpose()<<endl;
    // cout << "state:  " << mc.state.transpose()<<endl; 
    // cout << "stateFlag:  " << mc.stateFlag.transpose()<<endl;
    // cout << "leg:  " << mc.legCmdPos.transpose()<<endl;
    // cout << mc.global_v << endl;
    // cout << "imu:   "<< mc.imu_num.transpose()<<" "<< mc.target_taoz<<endl;
    // cout << "dis: "<<mc.distance.transpose()<<endl;
    oFile << mc.global_v << endl;
    // oFile << mc.presentCoMVelocity[0] << "," << mc.presentCoMVelocity[1] << "," << mc.presentCoMVelocity[2] << "," << mc.targetCoMVelocity[0] << "," <<  mc.targetCoMVelocity[1] <<"," << mc.targetCoMVelocity[2] << "," << mc.imu_num[0] << "," << mc.imu_num[1] << "," << mc.imuVel
    // [2] << endl;
  }
oFile.close();
delete robot;
return 0;
}
